//package legorobot;

import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;

/**
 *
 * @author Dan
 */
public class LouieControl {

    
    //The robot's sensors
    private TouchSensor frontTouchSensor = new TouchSensor(SensorPort.S1);
    
    private Motor[] motors;

    //Whether the robot will continue to accept commands
    private boolean acceptingCommands = true;
    private BTConnection btConn;
    private DataInputStream bt_dInStream;
    private DataOutputStream bt_dOutStream;
    private String[] stringsOnDisplay;
    
    //Define some contstants
    private final int MAX_LINES = 8; /* Max displayable lines on this NXT *
                                      * brick                             */
    
    /* All of the following must remain less than 16 characters, which is the
     * maximum amount of characters displayable per line on the NXT brick
     * If adding a new update display below, add a new one of these so we can
     * make sure that all strings used are within the 16 character limit      */
    private final String connectedString =        "Connected";
    private final String waitString =             "Waiting...";
    private final String connectionFailedString = "Conn failed";
    private final String invalidCmdString =       "Invalid Command";
    private final String getCmdFailedString =     "Get cmd Failed";
    private final String sendAckFailedString =    "Send Ack Failed";
    private final String sendReadyFailedString =  "Send Rdy Failed";
    private final String waitFailedString =       "Wait Failed";
    
    //Server communication strings
    private final String okString =               "OK";
    private final String readyString =            "ReadyForCmd";
    
    // A stack of recently used commands, useful for backtracking
    private CommandStack cmdStack;
    
    //TODO need to implement a command stack and a command class with data
    // about what to do when a certain command is received
    
    public LouieControl()
    {
        stringsOnDisplay = new String[MAX_LINES];
        for(int i = 0; i < MAX_LINES; i++)
            stringsOnDisplay[i] = "";
        
        cmdStack = new CommandStack();
        
        motors = new Motor[2];
        motors[0] = Motor.C;
        motors[1] = Motor.B;
        
        //Try to connect to a server... blocks until a connection is established
        while(!connect())
        {} //keep trying to connect
        
        /*
         * Using this command loop structure, bad errors will simply return from
         * the getOneCommand() method.  Fatal errors will change
         * acceptingCommands to false and then return, causing the robot to
         * shutdown.
         */
        while(acceptingCommands) {
            if(!getOneCommand()) {
                disconnect();
                backtrack();
                while(!connect()){}
                    //continuosly try to reconnect
            }//end-if
        }//end-while
        
        //TODO remove this later, unless we decide that when the robot gets
        //whatever we decide is a "fatal error" it should backtrack
        backtrack();
        
        //Graceful shutdown and return here.
        //updateDisplay(shuttingDownString);
    }
    
    /*
     * Retrieve one command from server.
     * Return false if something failed, so disconnect and try to reconnect.
     */
    private boolean getOneCommand()
    {
        String rawCmd;
        Command cmd;
        
        //Inform the server that we are ready for another cmd
        try{
            writeString(readyString);
        }
        catch(IOException e) {
            //Error, graceful recovery
            updateDisplay(sendReadyFailedString);
            return false;
        }
        
        //Read command from server
        try{
            rawCmd = readString();
        }
        catch(IOException e) {
            //Error, graceful recovery
            updateDisplay(getCmdFailedString);
            return false;
        }
        
        //Confirm reception to server
        try{
            writeString(okString);
        }
        catch(IOException e) {
            //Error, graceful recovery
            updateDisplay(sendAckFailedString);
            return false;
        }
        
        //Create command
        if(rawCmd.equals(MoveForwardCmd.getStringRep())) {
            cmd = new MoveForwardCmd(this);
        }
        else if(rawCmd.equals(MoveBackwardCmd.getStringRep())) {
            cmd = new MoveBackwardCmd(this);
        }
        else if(rawCmd.equals(TurnRightCmd.getStringRep())) {
            cmd = new TurnRightCmd(this);
        }
        else if(rawCmd.equals(TurnLeftCmd.getStringRep())) {
            cmd = new TurnLeftCmd(this);
        }
        else if(rawCmd.equals("disconnecting")) {
            cmd = new NullCmd(null); // prevent netbeans from complaining
            updateDisplay("Server Gone");
            return false;
        }
        else if(rawCmd.equals("shutdown")) {
            cmd = new NullCmd(null); //Null cmd doesn't need a LouieControl obj
            System.exit(0);
            return true;
        }
        else if(rawCmd.equals("backtrack")) {
            backtrack();
            return true;
        }
        else {
            // Invalid command received: Gracefully try to get another command
            updateDisplay(invalidCmdString);
            return true; //this returns true to say keep going
        }
        
        //Execute command.  If we decided to shut down, then the NullCmd will
        //execute, effectively doing nothing
        cmd.execute();
        
        if(frontTouchSensor.isPressed()){
            Command react;
            react = new MoveBackwardCmd(this);
            react.execute();
            cmd = new NullCmd(null);//we don't want to push the move
                                    //forward command onto the stack
        }
        
        //add created command to stack
        //Even though it is a null cmd, we don't want to push it onto the stack,
        //lest we risk breaking the backtrack code
        if(!(cmd.getName().equals("null")))
            cmdStack.pushCommand(cmd);
        
        //continue on to get next command
        return true;
    }
    
    private boolean waitForConn()
    {
        updateDisplay(waitString);
        
        //Accept incoming bluetooth requests from the Server
        btConn = Bluetooth.waitForConnection();
        
        if(btConn == null)
            return false;
        return true;
    }
    
    private boolean establishDataStreams()
    {
        try {
            bt_dInStream = btConn.openDataInputStream();
            bt_dOutStream = btConn.openDataOutputStream();
        }catch(IOException e){
            updateDisplay(connectionFailedString);
            return false;
        }
        return true;
    }
    
    private boolean connect() {
        //Wait for bluetooth connection from server
        if(!waitForConn())
            return false;

        //Establish input and output data streams to server
        if(!establishDataStreams())
            return false;
        
        updateDisplay(connectedString);
        return true;
    }//end-connect
    
    private void disconnect() {
        btConn.closeStream();
        btConn.close();
    }//end-disconnect
    
    public Motor getLeftMotor() {
        return motors[0];
    }
    
    public Motor getRightMotor() {
        return motors[1];
    }
    
    public void waitFor(int ms) {
        try {
            Thread.sleep(ms);
        }catch(InterruptedException e) {
            updateDisplay(waitFailedString);
        }
    }
    
    /*
     * Display a one-line string on the display
     */
    public void updateDisplay(String s) {
        //clear display
        LCD.clear();
        
        //shift all previous strings over, discarding the last one
        for(int i = (MAX_LINES - 2); i >= 0; i--)
            stringsOnDisplay[i+1] = stringsOnDisplay[i];
        
        //Set first line to new command
        stringsOnDisplay[0] = s;
        
        //Draw the new command plus the 7 oldest commands
        for(int i = 0; i < MAX_LINES; i++)
            LCD.drawString(stringsOnDisplay[i], 0, i);
        
        //refresh display
        LCD.refresh();
    }
    
    /**
     * Read a Cmd object from the server.  We'll have to convert this
     * a robot command.
     * 
     * @return
     * @throws java.io.IOException
     * @throws java.lang.ClassNotFoundException
     */
    /*public Cmd readCommand() throws IOException, ClassNotFoundException {
        Cmd c;
        
        c = (Cmd) bt_dInStream.readObject();
        
        return c;
    }*/
    
    /*
     * Send a string to the server, one character at a time
     */
    public boolean writeString(String s) throws IOException{
        
        if(s.length() < 1 || s == null)
            return false;
        
        for(int i = 0; i < s.length(); i++) {
            bt_dOutStream.writeChar(s.charAt(i));
            bt_dOutStream.flush();
        }
        bt_dOutStream.writeChar('*');
        bt_dOutStream.flush();
        
        return true;
    }
    
    /*
     * Read a string from the server, one character at a time
     */
    public String readString() throws IOException {
        char c;
        String temp = "";

        while((c = bt_dInStream.readChar()) != '*')
            temp += c;
        
        return temp;
    }
    
    /*
     * Remove commands from the stack until it is empty, trying to back up
     * the robot
     */
    private void backtrack() {
        updateDisplay("Backtracking...");
        Command c = cmdStack.popCommand();
        while ( (c != null) && (!c.getName().equals("null")) ) {
            c.getOpposite().execute();
            c = cmdStack.popCommand();
        }
    }
    
    /*
     * Simply creates an instance of LouieControl to control the lego robot.
     */
    public static void main(String args[]) throws Exception
    {
        //Create a new robot controller and let it go
        new LouieControl();
    }
}
